#' Add node to spatial lines object
#'
#' @inheritParams route_split
#' @param sln A spatial lines (`sfNetwork`) object created by `SpatialLinesNetwork`
#' @export
#' @examples
#' sample_routes <- routes_fast_sf[2:6, NULL]
#' sample_routes$value <- rep(1:3, length.out = 5)
#' rnet <- overline2(sample_routes, attrib = "value")
#' sln <- SpatialLinesNetwork(rnet)
#' p <- sf::st_sfc(sf::st_point(c(-1.540, 53.826)), crs = sf::st_crs(rnet))
#' sln_nodes <- sln2points(sln)
#' sln_new <- sln_add_node(sln, p)
#' route <- route_local(sln_new, p, sln_nodes[9, ])
#' plot(sln)
#' plot(sln_nodes, pch = as.character(1:nrow(sln_nodes)), add = TRUE)
#' plot(route$geometry, lwd = 9, add = TRUE)
sln_add_node <- function(sln, p) {
  rnet_new <- rnet_add_node(sln@sl, p)
  SpatialLinesNetwork(rnet_new)
}
#' Extract nodes from route network
#'
#' @inheritParams route_split
#' @param rnet A route network of the type generated by `overline()`
#' @export
#' @examples
#' rnet_get_nodes(route_network_sf)
rnet_get_nodes <- function(rnet, p = NULL) {
  rnet_start_end_points <- line2points(rnet)
  dupes <- duplicated(sf::st_geometry(rnet_start_end_points))
  return(sf::st_geometry(rnet_start_end_points[!dupes, ]))
}
#' Add a node to route network
#'
#' @inheritParams route_split
#' @param rnet A route network of the type generated by `overline()`
#' @export
#' @examples
#' sample_routes <- routes_fast_sf[2:6, NULL]
#' sample_routes$value <- rep(1:3, length.out = 5)
#' rnet <- overline2(sample_routes, attrib = "value")
#' p <- sf::st_sfc(sf::st_point(c(-1.540, 53.826)), crs = sf::st_crs(rnet))
#' r_split <- route_split(rnet, p)
#' plot(rnet$geometry, lwd = rnet$value * 5, col = "grey")
#' plot(p, cex = 9, add = TRUE)
#' plot(r_split, col = 1:nrow(r_split), add = TRUE, lwd = r_split$value)
#' @export
#'
rnet_add_node <- function(rnet, p) {
  if (sf::sf_extSoftVersion()["GEOS"] > "3.6.1") {
    i_nearest <- sf::st_nearest_feature(p, rnet)
  } else {
    i_nearest <- route_nearest_point(r = rnet, p = p, id_out = TRUE)
  }
  r <- rnet[i_nearest, ]
  rnet_except_r <- rnet[-i_nearest, ]
  r_split <- route_split(r, p)
  rbind(rnet_except_r, r_split)
}

#' Split route in two at point on or near network
#'
#' @param r An `sf` object with one feature containing a linestring geometry to be split
#' @param p A point represented by an `sf` object the will split the `route`
#'
#' @return An sf object with 2 feature
#' @export
#'
#' @examples
#' sample_routes <- routes_fast_sf[2:6, NULL]
#' r <- sample_routes[2, ]
#' p <- sf::st_sfc(sf::st_point(c(-1.540, 53.826)), crs = sf::st_crs(r))
#' plot(r$geometry, lwd = 9, col = "grey")
#' plot(p, add = TRUE)
#' r_split <- route_split(r, p)
#' plot(r_split, col = c("red", "blue"), add = TRUE)
route_split <- function(r, p) {
  pmat <- sf::st_coordinates(p)
  r_coordinates <- sf::st_coordinates(r)
  rmat <- nabor::knn(data = r_coordinates[, 1:2], query = pmat, k = 1)
  p_in_r <- sf::st_sfc(sf::st_point(r_coordinates[rmat$nn.idx, 1:2]), crs = sf::st_crs(r))
  route_split_id(r = r, p = p_in_r)
}
#' Split route based on the id or coordinates of one of its vertices
#' @inheritParams route_split
#' @param id The index of the point on the number to be split
#' @export
#' @examples
#' sample_routes <- routes_fast_sf[2:6, 3]
#' r <- sample_routes[2, ]
#' id <- round(n_vertices(r) / 2)
#' r_split <- route_split_id(r, id = id)
#' plot(r$geometry, lwd = 9, col = "grey")
#' plot(r_split, col = c("red", "blue"), add = TRUE)
route_split_id <- function(r, id = NULL, p = NULL) {
  if (is.null(id) && is.null(p)) {
    id <- round(n_vertices(r) / 2)
  }
  if (is.null(p)) {
    p <- sf::st_sfc(sf::st_point(sf::st_coordinates(r)[id, 1:2]))
  }
  r_new_geometry_collection <- lwgeom::st_split(r, p)
  sf::st_collection_extract(r_new_geometry_collection, "LINESTRING")
}
#' Find nearest route to a given point
#'
#' This function was written as a drop-in replacement for `sf::st_nearest_feature()`,
#' which only works with recent versions of GEOS.
#'
#' @param r The input route object from which the nearest route is to be found
#' @param p The point whose nearest route will be found
#' @param id_out Should the index of the matching feature be returned? `FALSE` by default
#' @export
#' @examples
#' r <- routes_fast_sf[2:6, NULL]
#' p <- sf::st_sfc(sf::st_point(c(-1.540, 53.826)), crs = sf::st_crs(r))
#' route_nearest_point(r, p, id_out = TRUE)
#' r_nearest <- route_nearest_point(r, p)
#' plot(r$geometry)
#' plot(p, add = TRUE)
#' plot(r_nearest, lwd = 5, add = TRUE)
route_nearest_point <- function(r, p, id_out = FALSE) {
  r_coordinates <- sf::st_coordinates(r)
  p_coordinates <- sf::st_coordinates(p)
  rmat <- nabor::knn(data = r_coordinates[, 1:2], query = p_coordinates[, 1:2, drop = FALSE], k = 1)
  r_nearest_id <- r_coordinates[rmat$nn.idx, 3]
  if (id_out) {
    return(r_nearest_id)
  } else {
    r[r_nearest_id, ]
  }
}
